#define MIDPULSE	1450
#define MAXPULSE	2300 //higher pulse corresponds to anti-clockwise motion
#define MINPULSE	600
#define DURATION	40
#define INCREMENT	850
#define PERIOD		20

struct servo_ {
  	int dir;
  	int pin;
};

int servoID = -1; 
servo_ servo[5] = { -1,3,-1,5,-1,6,-1,9,-1,10};

//servo[0].pin = 3; //pinky finger
//servo[1].pin = 5; //ring finger
//servo[2].pin = 6; //middle finger
//servo[3].pin = 9; //index finger
//servo[4].pin = 10; //thumb

//servo[0].dir = -1;//-1 corresponds to extended finger
//servo[1].dir = -1;
//servo[2].dir = -1;
//servo[3].dir = -1;
//servo[4].dir = -1;

int keypress = -1;

void setup() 
{
 	// put your setup code here, to run once:
	Serial.begin(9600);
	pinMode(servo[0].pin, OUTPUT);
	pinMode(servo[1].pin, OUTPUT);
	pinMode(servo[2].pin, OUTPUT);
	pinMode(servo[3].pin, OUTPUT);
	pinMode(servo[4].pin, OUTPUT);
}

void loop() 
{
 	// put your main code here, to run repeatedly:
	while(Serial.available() > 0)
	{
		keypress = Serial.read();
		Serial.print("keypress: ");
		Serial.println(keypress);

		switch(keypress)
		{
			case 49: 
						servoID = 0;				
						servo[servoID].dir = -1*servo[servoID].dir;
						moveServo(servoID);	
						break;		
			case 50:
						servoID = 1;				
						servo[servoID].dir = -1*servo[servoID].dir;
						moveServo(servoID);
						break;
			case 51:
						servoID = 2;				
						servo[servoID].dir = -1*servo[servoID].dir;
						moveServo(servoID);
						break;
			case 52:
						servoID = 3;				
						servo[servoID].dir = -1*servo[servoID].dir;
						moveServo(servoID);
						break;
			case 53:
						servoID = 4;				
						servo[servoID].dir = -1*servo[servoID].dir;
						moveServo(servoID);
						break;
			case 114:	
						for(servoID = 0; servoID < 5;servoID++)
						{
							if(servo[servoID].dir == -1)
							{
								servo[servoID].dir = -1*servo[servoID].dir;
								moveServo(servoID);
							}
						}
						break;
			case 112:	
						for(servoID = 0; servoID < 5;servoID++)
						{
							if(servo[servoID].dir == 1)
							{
								servo[servoID].dir = -1*servo[servoID].dir;
								moveServo(servoID);
							}
						}
						break;
			case 115:
						for(servoID = 0; servoID < 5;servoID++)
						{
							if( (servoID != 2) && (servoID != 3) )
							{
								if(servo[servoID].dir == -1)
								{
									servo[servoID].dir = -1*servo[servoID].dir;
									moveServo(servoID);
								}
							}else
							{
								if(servo[servoID].dir == 1)
								{
									servo[servoID].dir = -1*servo[servoID].dir;
									moveServo(servoID);
								}
							}
						}
						break;
			default:
						Serial.println("Incorrect key pressed!");

		}
		
	}
}
void moveServo(int ID)
{
	if( (ID > -1) && (ID < 5))
	{
		Serial.print("Moving servo ");
		Serial.print(ID);
		Serial.print(" to dir: ");
		Serial.println(servo[ID].dir);
		for(int i=0;i<DURATION;i++)
		{
			digitalWrite(servo[ID].pin, HIGH);
			delayMicroseconds( ( MIDPULSE + (servo[ID].dir*INCREMENT) ) );
			digitalWrite(servo[ID].pin, LOW);
			delay(PERIOD);
		}
	}else
	{
		Serial.println("Incorrect servo ID");
	}
}